////////////////////////////////////////////////////////////////////////////////
// SimuCab 2.0 car simulation
// Author SEDRUE JEAN-DAVID
// http://imaginesite.fr
// imaginesiteweb@gmail.com
//
// open source code, respect gpl license terms
// permission to copy,distribute and/or modify it.
// If you want to use it, please quote author and source. 
//
// Bug Correction by RacingMat , line 210.
////////////////////////////////////////////////////////////////////////////////
// sketchbook 0.26, arduino uno
// motor driver shield : monstermoto shield SPARKFUN
////////////////////////////////////////////////////////////////////////////////

#define BRAKEVCC 0
#define RV  2 //according to cable at 24/11/2011
#define FW  1 //according to cable at 24/11/2011
#define STOP 0
#define BRAKEGND 3
#define CS_THRESHOLD 253 //not use Sad
////////////////////////////////////////////////////////////////////////////////
#define pwmMin 128;
#define pwmMax 250;
#define motLeft 0
#define motRight 1
#define potL A5
#define potR A4
////////////////////////////////////////////////////////////////////////////////
//  DECLARATIONS
////////////////////////////////////////////////////////////////////////////////
/*  VNH2SP30 pin definitions*/
int inApin[2] = {7, 4};  // INA: Clockwise input
int inBpin[2] = {8, 9}; // INB: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int cspin[2] = {2, 3}; // CS: Current sense ANALOG input
int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)
int statpin = 13;
/* init position value*/
int DataValueL=125; //middle position
int DataValueR=125; //middle position
////////////////////////////////////////////////////////////////////////////////
// INITIALIZATION
////////////////////////////////////////////////////////////////////////////////
void setup()
{
        /* serial initialization */
        Serial.begin(115200);
        /* motor initialization */
        pinMode(statpin, OUTPUT);
        for (int i=0; i<2; i++)
        {
                pinMode(inApin, OUTPUT);
                pinMode(inBpin, OUTPUT);
                pinMode(pwmpin, OUTPUT);
        }
        // Initialize braked for motor
        for (int i=0; i<2; i++)
        {
                digitalWrite(inApin, LOW);
                digitalWrite(inBpin, LOW);
        }
        //  testpulse;
}
////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// Main Loop ////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void loop()
{
        int sensorL,sensorR;
        serialData();
        sensorR = analogRead(potR)/4;
        sensorL = analogRead(potL)/4; // scale 0-1000
        Serial.print(sensorL);
        Serial.print(" ; ");
        Serial.println(sensorR);
        motorMotion(motRight,sensorR,DataValueR);
        motorMotion(motLeft,sensorL,DataValueL);
}
////////////////////////////////////////////////////////////////////////////////
// Procedure: wait for complete trame
////////////////////////////////////////////////////////////////////////////////
void serialData()
{
        byte Data[3]={
                '0','0','0'    };
        // il faut limiter le temps pour garder le controle des moteurs
        if (Serial.available()>2){ //wait two packets
                //test if order  R or L data type
                Data[0]=Serial.read();
                if (Data[0]=='R'){ // ordre connu ?, si non, je verrai au prochain tour !
                        Data[1]=Serial.read();
                        Data[2]=Serial.read();
                        DataValueR=NormalizeData(Data);
                }
                if (Data[0]=='L'){ // ordre connu ?, si non, je verrai au prochain tour !
                        Data[1]=Serial.read();
                        Data[2]=Serial.read();
                        DataValueL=NormalizeData(Data);
                }
        }
        if (Serial.available()>16) Serial.flush();
}
////////////////////////////////////////////////////////
void motorMotion(int numMot,int actualPos,int targetPos)
////////////////////////////////////////////////////////
{
        int Tol=4;
        int difference;
        int pwm;
        int securityLow=20;
        int securityHigh=200;
        int brakeDistance=16;
        boolean move=true;
        // limits values
        //software limits, but inertia brake consideration.
        if (targetPos<securityLow+brakeDistance)   targetPos=(securityLow+brakeDistance);    
        if (targetPos>securityHigh-brakeDistance)  targetPos=(securityHigh-brakeDistance);
        // calculate speed about distance
        difference=abs(targetPos-actualPos);
        if (difference<=Tol) pwm=0;  // too near to move
        if (difference>Tol) pwm=168;
        if (difference>50) pwm=200;
        if (difference>75) pwm=225;
        if (difference>100) pwm=255;
        // pwm=255;  //test !
        // pay attention for security for Right motor!!
        if (actualPos>securityHigh || actualPos<securityLow) move=false; //mechanicals limits
        // too near to move
        if (difference<= Tol) move=false; // too near to move  
        // clockwise - go up!
        if (actualPos<targetPos && move) {
                motorGo(numMot, FW, pwm);
        }
        // anti-clockwise - do down!
        if (actualPos>targetPos && move) motorGo(numMot, RV, pwm);
        // stop !
        if (move==false) motorOff(numMot);// STOP
}
////////////////////////////////////////////////////////////////////////////////
void motorOff(int motor){
        ////////////////////////////////////////////////////////////////////////////////
        digitalWrite(inApin[motor], LOW);
        digitalWrite(inBpin[motor], LOW);
        analogWrite(pwmpin[motor], 0);
}
////////////////////////////////////////////////////////////////////////////////
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
////////////////////////////////////////////////////////////////////////////////
{
        if (motor <= 1)
        {
                if (direct <=4)
                {
                        // Set inA[motor]
                        if (direct <=1)
                        digitalWrite(inApin[motor], HIGH);
                        else
                        digitalWrite(inApin[motor], LOW);
                        // Set inB[motor]
                        if ((direct==0)||(direct==2))
                        digitalWrite(inBpin[motor], HIGH);
                        else
                        digitalWrite(inBpin[motor], LOW);
                        analogWrite(pwmpin[motor], pwm);
                }
        }
}
////////////////////////////////////////////////////////////////////////////////
// testPot
////////////////////////////////////////////////////////////////////////////////
void testPot(){
        Serial.print("PotR : ");
        Serial.print(analogRead(potR));
        Serial.print(" ; PotL : ");
        Serial.println(analogRead(potL));
        delay(250);
}
////////////////////////////////////////////////////////////////////////////////
void testpulse(){
        int pw=120;
        while (true){
                motorGo(motLeft, FW, pw);
                delay(250);    
                motorOff(motLeft);
                delay(250);    
                motorGo(motLeft, RV, pw);
                delay(250);    
                motorOff(motLeft);  
                delay(500);    
                motorGo(motRight, FW, pw);
                delay(250);    
                motorOff(motRight);
                delay(250);    
                motorGo(motRight, RV, pw);
                delay(250);    
                motorOff(motRight);  
                Serial.println("testpulse pwm:80");  
                delay(500);
        }
}
////////////////////////////////////////////////////////////////////////////////
// Function: convert Hex to Dec
////////////////////////////////////////////////////////////////////////////////
int NormalizeData(byte x[3])
////////////////////////////////////////////////////////////////////////////////
{
        int result;
        //only a LSB and Carrier Return or 'L' or 'R' in case of value below 16 (ie one CHAR and not 2)
        if ((x[2]==13) || (x[2]=='R') || (x[2]=='L')) 
        {
                x[2]=x[1];  //move MSB to LSB
                x[1]='0';     //clear MSB
        }
        for (int i=1; i<3; i++)
        {
                if (x>47 && x<58 ){//for xA to xF
                        x=x-48;
                }                    
                if (  x>64 && x<71 ){//for x0 to x9
                        x=x-55;            
                }
        }
        result=(x[1]*16+x[2]);
        return result;
}
